본 포스팅은 아래의 MathWorks 김종헌 프로님의 Path Planning 시리즈 중 아래의 비디오를 정리한 것입니다.

How Autonomous Mobile Robot Systems Work?

자율 모바일 로봇 시스템은 상당히 복잡한 알고리즘들로 이루어져 있다.

모바일 로봇이 동작하는 주변 환경에 대한 정보를 여러 가지 센서를 통해서 얻어오고 (Sense), 이 센서 정보를 가지고서 장애물이 어디에 존재를 하고 그 중에 동적인 장애물이 있는지 있다면 그 물체는 어디서 와서 어디로 가고 있는지 그리고 지금 나는 이 공간상에 어디에 위치해 있는지 등 내 로봇의 상황을 인지한다 (Perception) 그렇게 내 주변 상황이 인지가 되고 나면 그 인식된 정보를 기반으로 해서 현재 있는 위치에서부터 목표 지점까지 어떻게 움직일 것인가에 대한 계획한다. (Planning)

그러면 이제는 주변 상황을 계속 확인해가면서 만들어진 경로를 따라가는 제어 명령을 만들고 이를 로봇에 전달한다. (Control) 그럼 모바일 로봇이 명령에 따라서 움직이게 된다. (Vehicle) 이 움직에 따라 환경 내에서 로봇의 위치가 변하게 된다. (Environment) 이것으로 closed loop이 완성이 되며, 이 과정이 반복되면 최종적으로 로봇이 목표위치에 도착할 수 있다.

여기서 바로 목표지점까지 어떻게 갈것인가를 결정하는 부분이 바로 path planning 이다.


그림 1. 모바일 로봇이 움직이게 되는 방식

What is Path Planning?

Path Planning을 조금 더 엄밀하게 정의하자면, “주어진 환경 안에서 두 개의 로봇 상태, 시작 상태와 목표 상태를 연결하는 내 로봇이 실현 가능한 경로를 만들어 내는 작업” 이라고 말할 수 있다.

운전을 할 때 전진과 후진이 다르듯이, 여기서 상태란 위치 뿐만 아니라 로봇의 방향을 함께 고려하는 것이며, 아래 그림에서와 같이 ($x_a, y_a, \theta_a$)로 로봇의 상태를 표현할 수 있다.

경로를 계획할 때는 아래와 같은 프로세스를 거친다. 우선, 아래 그림 2에서 볼 수 있는 것 처럼 주변 환경 정보를 읽어온다. 이 과정은 그림 1에서 보았던 Perception 파트에서 수행하게 되는 것이다. 이제 내가 원하는 조건(혹은 목표)에 맞춰 경로 계획을 실행하게 된다. 여기서 이 조건(혹은 목표)은 충돌을 회피하는 것이라던지, 장애물로부터 얼마만큼의 거리를 유지하고 싶다던지, 최소한의 에너지로 이동해야 한다던지, 혹은 최대한 부드러운 경로로 움직여야 한다던지 등의 조건을 수학적으로 표현해주는 것이다.


그림 2. Perception 과정을 통해 얻은 주변 정보와 고려할 수 있는 조건들

그런데, Path Planning이라는 것이 알고리즘의 특징에 따라 완벽하게 원하는 패스를 한번에 얻지 못할 수도 있다. 아래는 조건(혹은 목표)를 만족시키는 Path 중에 하나인데, 보면 부드럽지 못한 형태를 갖고 있는 것을 볼 수 있다.


그림 3. 조건을 만족하는 경로의 예

그림 3에서 얻은 경로를 조금 최적화 해주어서 제시한 조건을 더 잘 만족하는 경로를 얻을 수도 있게 되는 것이다. 아래 그림 4는 그림 3에 비해 좀 더 부드러운 경로를 갖게 된 경우를 표현한 것이다.


그림 4. 조건을 더 잘 만족할 수 있도록 최적화된 경로의 예

Types of Path Planning Algorithms

Path Planning에 사용되는 알고리즘은 몇가지 기준에 따라 분류될 수 있다. 이 분류 기준에 따라 내가 개발하는 로봇은 이런 특징을 가지고 있으니 이 알고리즘 사용하면 되겠구나 하고 Path Planning 알고리즘을 선택하기 때문에 패스 플래너를 개발하는데 있어 이 기준은 중요할 수 있다. 참고로, 여기서 설명하는 이 분류 기준이 “학술적으로” 아주 정확하지는 않을 수 있다는 점은 언급해두고자 한다.

환경을 표현하는 방식에 따른 구분

일단 지금 Path Planning을 할 공간을 어떻게 표현하느냐에 따라서 먼저 구분을 할 수 있다. 상태 공간을 이산화를 시켜서 이산화된 공간 안에서 탐색을 하는 방식인 discrete planning과 이산화되지 않은 연속된 공간 안에서 경로를 탐색하는 continuous planning 방식으로 구분할 수 있다.


그림 5. 상태공간을 이산적/연속적으로 표현하는지에 따른 구분

일단 discrete planning 같은 경우에는 공간을 이산화 시켜 한정된 공간 안에서 Planning을 수행하게 된다. 이러한 공간안에서 로봇이 가질 수 있는 상태는 유한하기 때문에 특별히 로봇의 움직임을 제한하는 운동 방정식을 사용할 필요도 없고, 고려할 방법도 없다. 대표적인 discrete planning 방식의 알고리즘으로 A* algorithm을 생각할 수 있다.

continuous planning은 이와 반대로 공간안에서 로봇이 갖을 수 있는 상태가 무한하기 때문에 가능성 있는 모든 상태를 검색할 수 없다. 따라서, 연속된 공간을 대표하는 몇가지 샘플을 찾고 이 샘플된 상태가 실제로 로봇이 도달 가능한 상태인지 확인하기 위해 로봇의 운동학 혹은 동역학적 모델을 만들어 적용하게 된다. Continuous planning에는 수많은 algorithm이 있지만 A*를 연속 공간에서 사용하기 위해 변형된 형태인 Hybrid A*가 가장 대표적인 continuous planning 알고리즘의 예라고 할 수 있다.

검색 방식에 따른 구분

그 다음은 공간에서 시작점부터 목표지점까지 최적경로일 가능성이 높은 곳부터 점진적으로 경로를 찾아나가는 방법인 Search-based planning과 그리고 공간을 랜덤하게 샘플링을 해서 그 샘플링한 상태들을 연결해 가면서 연결할 두 상태사이에 장애물이 있으면 연결하지 않고, 없으면 연결하는 식으로 최적 경로를 생성하는 Sampling-based planning으로 구분할 수 있다.

이번에는 검색 방식에 대한 분류에 따라 특징을 살펴보자. 경로 계획을 할 때 경로를 얻어내는 방식에 따라 search기반, 샘플링 기반, 최적화 기반으로 구분되는데 Search 기반은 현재 검색 위치까지 도달하는데 필요한 비용과 앞으로 목표지점까지 얼마나 남았는지 heuristic 값을 구해 그 값이 가장 작은 지점들을 찾도록 시작점부터 점진적으로 검색하는 방법이다. 이 방법은 단순하기 때문에 저차원의 덜 복잡한 환경에서는 속도도 빠르고 해가 존재한다면 시간이 얼마가 걸리든 반드시 그 해를 찾을 수 있다고 보장한다.

그림 6. Search-based Planning

샘플링 기반의 경우에는 탐색 공간에 임의의 점을 찍어서 그 임의의 점에서 지금 이미 가지고 있는 트리에 장애물과의 충돌없이 연결가능한지 확인하면서 검색 트리를 확장해 나간다. 샘플링 기반 플래너는 상태의 샘플링을 임의로 하기 때문에 일반적인 경우에는 굉장히 복잡하거나 높은 차원의 공간에서도 굉장히 효율적으로 작동한다. 하지만, 굉장히 좁은 통로만이 유일한 경로 이고 여기에 상태 샘플링이 안되면 훨씬 더 오래 시간이 걸릴 수도 있다. 그래서 해를 꼭 반드시 찾아준다라는 보장도 없고, 얻어진 경로도 임의의 상태들을 연결했기 때문에 최적화된 경로라고 할 수도 없다. 대표적으로 RRT, RRT*, PRM 등의 알고리즘이 샘플링 기반의 알고리즘이다.

그림 7. Sampling-based Planning

최적화 기반 플래너는 제어라고 보는 경우도 있고 플래너라고 보는 경우도 있는데 그런 논란에서 일단 한 발자국 뒤로 물러서서 여기서는 일단 플래너의 하나로 보도록 하자. 이 방법은 내 로봇이 처해있는 환경 그리고 내 로봇의 다이나믹스, 퍼포먼스, 그외 여러 제한 조건들을 수식화 해서 코스트 펑션으로 만들고 이를 최적화 하는 방식으로 경로를 생성해낸다.. 어디에 어떤 장애물이 있을지 모르는 불확실한 환경에서 온라인 패스 플래너로써 사용하는 경우가 많다. MPC(Model Predictive Control)가 대표적인 최적화 기반 플래너라고 볼 수 있다.


그림 8. Optimization-based Planning

계층 구조적인 구분 (Global, Local, …)

다른 구분 방법으로는 Path Planning을 수행하는 스케일이 거시적인 맵이면 global planning, global planning 결과에서 국부적으로 들어가서 일부 구간에서 플래닝 수행하면 local planning으로 구분하기도 한다.

계층 구조적으로 패스 플래너를 또 구분을 해보자면 원래 넓고 복잡한 공간에 대해서 한 번에 최적 패스를 구하는 것이 계산상으로 굉장히 비효율적이고, 내 로봇이 그 곳에 위치하는 시점에 그 곳에 어떤 장애물이 있을지 정보가 없기 때문에 많이 접근하는 방식이 계층 구조적으로 각 단계별로 스케일을 달리해가며 패스플래닝을 수행함으로써 그 복잡도를 낮추고 로봇에 실장을 했을 때 프로세서가 결과를 효율적으로 계산할 수 있도록 패스 Planner를 나누게 된다.

그래서 각 단계로 이렇게 글로벌 Planner, behavior Planner, 로컬 Planner 보통 이런 식으로 나누게 된다.


그림 9. 계층적인 Path Planning 방법

글로벌 플래너는 환경에 대해서 이미 알고 있는 정보들을 기반으로 충돌이 존재하지 않는 웨이 포인트의 집합으로서의 경로를 생성을 하게 된다. 그러니까 자율주행 같은 경우는 어디에 빌딩이 있고, 어디에 우체통이 있고, 어디가 신호등이 있고, 여기에 road network 정보로 어디에 차선이 있고 어디에 교차로가 존재를 하고 이런 식의 정보들을 기반으로 해서 플래닝하게 되는데 이때는 보통 route planning이라고 부르게 된다. 아니면 공장 같은 경우에는 여기에 어디에 선반이 있고, 어디에 조립하는 라인이 있고 이런 등등의 정보들을 맵 형태로 만들어서 이 위에서 목적지를 입력하면 waypoint들의 조합인 경로가 출력된다. 이 작업은 굳이 꼭 리얼 타임이 아니고 로봇 내에서 반드시 수행할 필요가 없기 때문에 오프라인에서 수행을 하는 경우가 많다.

Behavior Planning 단계에서는 앞에서 만든 웨이 포인트들을 따라갈 때 센서 정보를 기반으로 주변 상황과 로봇이 어떻게 상호작용을 해야지 좀 더 안전하고 빠르게 효율적으로 도달할 수 있는가 전략을 결정하게 된다. 쭉 경로를 따라가고 있는데 앞에 이동 장애물이 존재를 할 때 내가 잠깐 멈췄다 가야 되는가 아니면 오른쪽 혹은 왼쪽으로 돌아서 가야 되는가 이런 거시적인 움직임을 결정을 하게 된다. 그래서 주변의 정보들을 Planner에 전략이 결정이 되게 된다. 일반적으로 이런 결정을 위해 여러 조건들이 조합이 되기 때문에 돼서 결정이 돼야 되기 때문에 흔히들 finite state 머신을 사용하여 이런 플래너를 설계한다. 이 방식을 활용하면 시각적으로 가독성이 높아 복잡한 조건들을 조합해서 결과를 얻어낼 수 있더. 경우에 따라서 요즘에는 딥러닝이 많이 적용되기도 한다.

어찌 됐건 이런 behavior가 만들어지면 이 behavior와 글로벌 플래너에서 나온 웨이포인트를 조합해서 이동할 때 맵에 기재되지 않았던 어떤 장애물을 회피해서 동작을 하기 위해 글로벌 패스 플래너의 결과를 조정을 하는 형태를 로컬 Planner라고 얘기할 수 있다. 로컬 플래닝 같은 경우에는 현재의 센서 데이터를 기반으로 해서 그때그때 실제 로봇의 움직임을 수정을 하기 때문에 로봇 안에서 온라인으로 도는 경우가 더 많다. 어찌되었던 로컬플래너를 통하면 최종적으로 로봇의 주행 궤적이 생성되게 된다.

MATLAB에서 지원하는 Path Planning 알고리즘

MATLAB에서 지원하는 Path Planning 알고리즘의 구분


그림 10. 매트랩에서 지원하는 Path Planning 알고리즘의 구별

그래서 이런 여러 가지 분류 기준에 따라서 Path Planning 알고리즘들을 구분을 할 수가 있는데, 매트랩에서 제공하는 패스 Planner 같은 경우에는 글로벌 Planner, 로컬 Planner, discrete planner, continuous planner, 탐색기반, 샘플링기반, 최적화 기반의 분류로 알고리즘들을 분류해볼 수 있다.

이러한 함수들에 대해서는 이후 시리즈 비디오에서 자세히 다루게 될 것이다.

Planner Layout

그럼 이런 플래너 함수들은 어떻게 구성이 돼 있을까? 기본적인 MATLAB에서 제공하는 Planner의 기본적인 구성은 그림 11과 같다.


그림 11. MATLAB의 planner 기본 구성

전체 큰 껍데기로서 플래너 오브젝트가 존재를 하고 그 안에 경로 계획에 필요한 기본적인 정보를 담는 몇가지 object를 입력으로 제공한다.

State Space

첫번째 확인할 내용은 state space이다. State Space 는 플래닝 실제 경로 계획을 할 공간을 정의한다. 여기서 로봇이 가질 수 있는 스테이트는 어떤 값들이 있는지를 정의하는데 Planner에서는 공간 검색이나 샘플링을 할 때 이 스테이트 표현 방식을 기반해서 스테이트를 결정을 하게 된다.


그림 12. MATLAB에서 지원하는 State Space

MATLAB에서 제공하는 state space는 x y 평면 위에서 헤딩 앵글을 스테이트 값으로 갖는 SE2가 있고, 3차원의 경우에는 x, y, z 좌표에 quaternion으로 방위를 표현하는 SE3, Nonholonomic 시스템을 표현하기 위해 Durbins 상태 공간 기반의 State Space, Reeds-Shepp Vehicle 모델 기반의 state space도 제공한다.

이 State Space 에 따라서 보시는 것처럼 각각의 샘플링된 스테이트들을 연결하는 방법이 달라지기 때문에 플래너를 구성하는 데 중요한 인자라고 볼 수 있다.

Validator

State Validator 는 플래너와 환경을 연결을 해주는 인터페이스 역할을 하게 된다. State validator는 로봇의 상태를 표현을 하는 스테이트 혹은 스테이트와 스테이트를 연결하는 모션 세그먼트가 있을 때, 이것들이 정의된 환경에서 충돌이 존재하는지 안 하는지 판단을 하는 역할을 한다. 동작하는 환경에 따라 그에 맞는 validator를 제공하고 있고 로봇의 스테이트가 장애물과 충돌이 있는지 없는지를 체크를 하는 isStateValid 메소드와 스테이트 사이를 연결하는 모션에서 충돌이 있는지 없는지를 체크하는 isMotionValid 두 개의 method를 제공한다.

이 두 개의 메소드는 플레너 내부에서 공간을 검색할 때 반복적으로 호출되어 사용함으로써 각 state나 motion에 충돌이 있는지 없는지 내부적으로 체크하게 된다.


그림 13. MATLAB에서 지원하는 Validator

Environment

Path Planning을 하기 위해서는 어떻게 주변 환경을 표현할지도 중요한데 환경을 구성하는 방식에도 여러 가지가 있다. 정적인 환경이냐, 동적인 환경이냐, 그리드 기반으로 이산화를 할 거냐 아니면 그래프 형태로 이산화를 할 거냐 2차원이냐 3차원이냐 등등에 따라 MATLAB에서는 여러 표현 방식을 제공을 하고 있다. 이런 부분도 추후 비디오 시리즈를 통해 나중에 좀 더 디테일하게 다루도록 할 예정이다.

그림 14. MATLAB에서 지원하는 다양한 환경 표현 방식

Five-Step Path Planning Workflow with MATLAB


그림 15. MATLAB을 이용한 Path Planning 의 Workflow

그럼 이런 구성을 가지고 있는 패스 플레너 함수를 이용해 실제로 패스 플레이닝을 하는 절차를 확인해보자.

  • Step 1: 처음에는 맵을 준비한다. 코드 상에서도 맵을 표현하는 조감도 이미지를 사용해 occupancy map을 만든다.
  • Step 2: 그리고 스테이트 스페이스를 정의하고, occupancy 맵을 사용해 충돌을 체크를 할 밸리데이터를 정의한다. 그리고 밸리데이터에서 맵을 참조하게 한다.
  • Step 3: 패스 플래너 내부에서 새로운 스테이트를 샘플링한다.
  • Step 4: 그 스테이트가 충돌이 있는지 없는지를 매 단계별로 반복적으로 수행한다.
  • Step 5: 각 스테이트를 조합해 패스가 만들어진다.

Step 3, 4, 5는 패스 플래너 오브젝트 안에서 자동으로 수행된다. Step 2에서 정의했던 스테이트 스페이스 오브젝트나 밸리데이터 오브젝트를 입력을 해서 패스 플레너 오브젝트를 생성을 하면 플레너 오브젝트의 plan이라는 메소드로 시작점과 끝점을 연결을 하는 패스 플레이닝을 수행하고 이 결과물을 아웃풋으로 출력을 할 수 있다.

아래는 MATLAB 코드이다.

% Step 1
load exampleMaps                                % load the map image
map = occupancyMap(simpleMap,10); % build occ with 0.1m resolution

% Step 2
ss = stateSpaceSE2;                     % create SE2 state space object
sv = validatorOccupancyMap(ss); % create occ based validator
sv.Map = map;                               % add map info.

% Step 3, 4 & 5
planner = plannerRRT(ss,sv);   % create RRT planner object
[pthObj,solnInfo] = planner.plan(start,goal); % plan the path

MATLAB/Simulink/Navigation Toolbox

MATLAB, Simulink, 그리고 주로 Path Planning 알고리즘을 제공을 하는 Navigation Toolbox 에서는 플래너 알고리즘, SLAM 알고리즘, 자기 위치를 확인하는 데 필요한 관성법을 알고리즘 등을 제공하고 있다. 그래서 이런 알고리즘을 기반으로 사용자의 어플리케이션 소프트웨어를 개발하고, 정량적인 매트릭을 통해서 얼마나 좋은 결과물을 만들었는지에 대해서 분석하고 비교할 수 있다. 최종적으로 내가 원하는 설계 목표에 도달하게 되면 실제로 코드 생성을 통해서 여러 형태의 타겟에 실장할 수 있는 기능 또한 제공하고 있다.

그림 16. MATLAB, Simulink, Navigation Toolbox에서 제공하는 전체적인 Path Planning 워크플로우

Technical Resources

Path Planning 분야 외의 로봇 개발을 위한 다른 내용이 궁금하다면 아래의 매스웍스 코리아에서 제공하는 모바일 로봇틱스 그리고 자율주행 웹 포털을 통해 정보를 얻어갈 수 있으니 참고하기 바란다.

MATLAB Mobile Robotics Web Portal


그림 17. MATLAB을 이용한 육상 이동 로봇 개발 Web Portal (링크)

MATLAB ADAS Web Portal


그림 18. MATLAB을 이용한 자율주행/ADAS 개발 Web Portal (링크)

MATLAB Onramp Series

매트랩 기초 사용법을 학습하고 싶은 경우 MathWorks 홈페이지 내의 Onramp 라는 무료 트레이닝 코스를 활용할 수 있다. Onramp는 웹상으로 진행하는 온라인 무료 교육으로, 컴퓨터에 매트랩을 설치 할 필요 없이 온라인으로 매트랩 관련된 여러 기초 내용을 학습할 수 있다.


그림 19. MATLAB을 무료로 배울 수 있는 Onramp 시리즈 (링크)